/*
智能化研究院-感知技术所, All rights reserved.
软件名称：激光雷达动态SLAM
软件功能：适应动态环境变化的SLAM，为下游提供装载机实时定位、姿态与速度，自带心跳，状态信号。
开发日期：2024.1
改版履历
                    版本|               日期|               开发人员|                       改版描述
                    V1                     2023.12          邓加成/张敏                 在静态SLAM基础上，开放基于体素映射的实时地图更新与维护功能
                    V2_1                2023.12          邓加成                            装载机剧烈冲击时点云波动剧烈，以此得到的点云集合不具有约束性，添加IMU约束模块，在IMU方差小于一定阈值时再开放地图更新功能
                    V2_2                2024.1             邓加成                           为减少CPU占用，添加将调试信息可选一键去除功能
                    V3                     2024.6             邓加成                           全场景地图更新存在发散问题，对场景中静态区域添加约束，利用体素映射数据结构特点完成约束添加（昆明建投版本）
                    V3_1                2024.6             邓加成                           添加SLAM的CPU占用可调节模块，可以选择性的调节SLAM的CPU占用为40%～100%之间
                    V3_2                2024.6             邓加成                           解除重定位3次限制，SLAM将反复执行重定位直到SLAM恢复
                    V3_3                2024.6             邓加成                           添加IMU数据判断模块，对IMU数据峰值进行滤除，对IMU队列异常进行判断和清空
                    V3_4                2024.6             邓加成                           更新lock锁，对所有线程的共享队列操作添加lock操作，避免队列调用出现指针冲突
                    （测试）V3_5                2024.6             邓加成                           清空IMU队列后需要重新对齐IMU和点云时间戳，添加时间戳对齐功能并加入IMU积分前置中
当前版本：V3_4
*/
#include <queue>
#include <thread>
#include <mutex>
#include <sched.h>
#include <pthread.h>

// include ros headers
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <eigen_conversions/eigen_msg.h>

// include pcl headers
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

// include eigen headers
#include <Eigen/Core>
#include <Eigen/Geometry>

// the lio lib
#include "lio.h"

// iVox the pointcloud storage structure
#include "ivox3d/ivox3d.h"

// pointcloud registration
#include "cld_reg.h"

// user messages
#include "dy_lio_pkg/rtk_msg.h"

using namespace std;

// input topic names
string motor_topic_name;
string imu_topic_name;
string pointcloud_topic_name;
string map_file_name;

// output topic names
string static_map_topic_name;
string undistort_pointcloude_topic_name;
string slam_pose_raw_topic_name;
string slam_path_raw_topic_name;
string slam_prv_raw_topic_name;

string slam_rtk_topic_name;
string slam_pose_topic_name;
string slam_path_topic_name;

string slam_safety_topic_name;
string slam_state_topic_name;

// component pointers
shared_ptr<slam::IVoxType> ivox{nullptr};
shared_ptr<slam::Lio> lioptr{nullptr};

slam::PointCloudType::Ptr cldptr_init{nullptr};

// motor messages
std::queue<Eigen::Vector3d> que_motor;
Eigen::Vector3d prev_motor(0.0, 0.0, 0.0); // (0): timestamp, (1): signed vel, (2): unsigned vel
Eigen::Vector3d cur_motor(0.0, 0.0, 0.0);

// thread controller
std::mutex mtx;
bool b_run = false;
bool b_fin = false;
std::queue<double> que_fail;

// kalman parameters
double kalman_x = 0.0;
double kalman_P = 1.0;
const double kalman_Q = 0.005;
const double kalman_R = 0.5;

// others
int n_state = 0;

/** transform ros message vector3 to Eigen vector3d (double)
 *
 * @param msg_vector the ros message vector3
 * @return the Eigen vector3d
 */
Eigen::Vector3d vector3d_msg_to_eigen(geometry_msgs::Vector3 msg_vector)
{
    Eigen::Vector3d vector;
    vector << msg_vector.x, msg_vector.y, msg_vector.z;
    return vector;
}

/** transform ros message matrix3x3 to Eigen matrix3d (double)
 *
 * @param msg_matrix the ros message matrix3x3
 * @return the Eigen matrix3d
 */
Eigen::Matrix3d matrix3d_msg_to_eigen(boost::array<double, 9> msg_matrix)
{
    Eigen::Matrix3d matrix;
    for (int i = 0; i < 9; i++)
    {
        matrix << msg_matrix[i];
    }
    return matrix;
}

/** callback function for motor message
 *
 * @param motor_msg the motor message
 * @return None
 */
void motor_callback(const geometry_msgs::PointStampedConstPtr &motor_msg)
{
    if (b_run)
    {
        mtx.lock();
        que_motor.push(Eigen::Vector3d(motor_msg->header.stamp.toSec(), motor_msg->point.y, motor_msg->point.z));
        // std::cout << "motor_data_buffer: " << que_motor.size() << std::endl;
        mtx.unlock();
    }
}

/** callback function for imu message, put the imu data into LIO
 *
 * @param imu_msg the imu message
 * @return None
 */
void imu_callback(const sensor_msgs::Imu::ConstPtr &imu_msg)
{
    if (b_run)
    {
        Eigen::Vector3d angular_velocity;
        angular_velocity = vector3d_msg_to_eigen(imu_msg->angular_velocity);
        Eigen::Vector3d acceleration;
        acceleration = vector3d_msg_to_eigen(imu_msg->linear_acceleration);
        if(angular_velocity.x()<1&&angular_velocity.y()<1&&angular_velocity.z()<1&&acceleration.x()<30&&acceleration.y()<30&&acceleration.z()<30)
        lioptr->PushImuData(imu_msg->header.stamp.toSec(), angular_velocity, acceleration);
        else
        std::cout<<"imu_data over limit! skip!"<<std::endl;
    }
}

/** callback function for pointcloud message, put the pointcloud data into LIO
 *
 * @param pointcloud_msg the pointcloud message
 * @return None
 */
void pointcloud_callback(const sensor_msgs::PointCloud2::ConstPtr &pointcloud_msg)
{
    if (b_run)
    {
        slam::PointCloudType::Ptr pointcloud = boost::make_shared<slam::PointCloudType>();
        pcl::fromROSMsg(*pointcloud_msg, *pointcloud);
        lioptr->PushPointcloud(pointcloud_msg->header.stamp.toSec(), pointcloud);
    }
    else
    {
        cldptr_init = boost::make_shared<slam::PointCloudType>();
        pcl::fromROSMsg(*pointcloud_msg, *cldptr_init);
    }
}

/** quaternion to Euler angel
 *
 * @param q input quaternion
 * @param roll output euler angel
 * @param pitch output euler angel
 * @param yaw output euler angel
 * @return None
 */
void quat2EulAng(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw)
{
    // roll (x-axis rotation)
    double sinr_cosp = 2.0 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    roll = atan2(sinr_cosp, cosr_cosp); // -M_PI ~ M_PI

    // pitch (y-axis rotation)
    double sinp = 2.0 * (q.w() * q.y() - q.x() * q.z());
    if (fabs(sinp) >= 1)
        pitch = copysign(M_PI / 2, sinp); // use (+-)M_PI/2 if out of range
    else
        pitch = asin(sinp); // -M_PI/2 ~ M_PI/2

    // yaw (z-axis rotation)
    double siny_cosp = 2.0 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
    yaw = atan2(siny_cosp, cosy_cosp); // -M_PI ~ M_PI
}

/** kalman filter
 *
 * @param u state transfers
 * @param z measurements
 * @return x, estimated state
 */
double kalman_filter(const double u, const double z)
{
    double x_ = kalman_x + u;
    double P_ = kalman_P + kalman_Q;
    double K = P_ / (P_ + kalman_R);
    kalman_x = x_ + K * (z - x_);
    kalman_P = (1.0 - K) * P_;
    return kalman_x;
}

/** initialize the topic names
 *
 * @param nh NodeHandle
 * @return None
 */
void init_parameters(ros::NodeHandle &nh)
{
    nh.param<string>("motor_topic_name", motor_topic_name, "/motorSpeed_topic");
    nh.param<string>("imu_topic_name", imu_topic_name, "/os_cloud_node/imu");
    nh.param<string>("pointcloud_topic_name", pointcloud_topic_name, "/os_cloud_node/points");
    // nh.param<string>("map_file_name",                   map_file_name,                    "/home/sany/rosbag2/20230302/nanqu_map1204_rotate_y.pcd");
    nh.param<string>("map_file_name", map_file_name, "/home/nvidia/wheelLoaderProgram/perception/240517kunming.pcd");
    // nh.param<string>("map_file_name",                   map_file_name,                    "/home/sany/rosbag2/20230614/0614yancheng02_voxel_map.pcd");

    nh.param<string>("static_map_topic_name", static_map_topic_name, "/slamStaticMap_topic");
    nh.param<string>("undistort_pointcloud_topic_name", undistort_pointcloude_topic_name, "/slamCldUndistort_topic");
    nh.param<string>("slam_pose_raw_topic_name", slam_pose_raw_topic_name, "/slamPoseRaw_topic");
    nh.param<string>("slam_path_raw_topic_name", slam_path_raw_topic_name, "/slamPathRaw_topic");
    nh.param<string>("slam_prv_raw_topic_name", slam_prv_raw_topic_name, "/slamPRVRaw_topic");

    nh.param<string>("slam_rtk_topic_name", slam_rtk_topic_name, "/slamPose_topic");
    nh.param<string>("slam_pose_topic_name", slam_pose_topic_name, "/slamPoseGeo_topic");
    nh.param<string>("slam_path_topic_name", slam_path_topic_name, "/slamPath_topic");

    nh.param<string>("slam_safety_topic_name", slam_safety_topic_name, "/slamSafety_topic");
    nh.param<string>("slam_state_topic_name", slam_state_topic_name, "/slamState_topic");
}

/** publish safety and state, thread function
 *
 * @param pub_safety Publisher
 * @param pub_state Publisher
 * @return None
 */
void publish_safety(ros::Publisher &pub_safety, ros::Publisher &pub_state)
{
    std::cout << "publish_safety() start" << std::endl;
    int n_keep = 0;
    geometry_msgs::Vector3Stamped msg_safety;
    geometry_msgs::Vector3Stamped msg_state;

    ros::Rate rate(10);
    while (ros::ok())
    {
        if (b_fin)
            break;

        if (n_keep)
            n_keep = 0;
        else
            n_keep = 1;
        msg_safety.header.stamp = ros::Time::now();
        msg_safety.vector.x = n_keep;
        pub_safety.publish(msg_safety);

        msg_state.header.stamp = ros::Time::now();
        msg_state.vector.x = n_state;
        pub_state.publish(msg_state);

        rate.sleep();
    }
    std::cout << "publish_safety() fin" << std::endl;
}

// zm--------------------------------------------------------------------------------------------------
void publish_ivox_cld(ros::Publisher &ivox_cld_publisher) ///// 按200ms的频率获取ivox中的点云进行发布查看
{
    ros::Rate loop_rate(4); // 设置发布频率为200Hz
    while (ros::ok())
    {
        slam::PointCloudType::Ptr ivox_pcl_cld(ivox->GetPointCloud()); // 耗时>100ms
        slam::PointCloudType::Ptr ivox_pcl_cld_filtered(new slam::PointCloudType);

        std::vector<slam::PointType> points;
        for (const auto &point : *ivox_pcl_cld)
        {
            if (point.z <= 4)
            {
                points.push_back(point);
            }
        }

        ivox_pcl_cld_filtered->width = points.size();
        ivox_pcl_cld_filtered->height = 1;
        ivox_pcl_cld_filtered->points.resize(ivox_pcl_cld_filtered->width * ivox_pcl_cld_filtered->height);
        for (size_t i = 0; i < points.size(); ++i)
        {
            ivox_pcl_cld_filtered->points[i] = points[i];
        }

        sensor_msgs::PointCloud2 ivox_cld_msg;
        pcl::toROSMsg(*ivox_pcl_cld_filtered, ivox_cld_msg);
        // pcl::toROSMsg(*ivox_pcl_cld, ivox_cld_msg);
        ivox_cld_msg.header.frame_id = "map";
        ivox_cld_publisher.publish(ivox_cld_msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
} // zm--------------------------------------------------------------------------------------------------

/** publish results, thread function
 *
 * @param pub_rtk Publisher
 * @param pub_pose Publisher
 * @param pub_path Publisher
 * @return None
 */
void publish_results(ros::Publisher &pub_rtk, ros::Publisher &pub_pose, ros::Publisher &pub_path)
{
    std::cout << "publish_results() start" << std::endl;

    const Eigen::Vector3d v_P_RTKL_O_SLAML(0.0 - 1.3, 0.4 - 0.449, -0.22397 - 2.7598);
    const Eigen::Vector3d v_t_SLAM2RTK(1.33082, -0.185613, 3.03216);
    const Eigen::Quaterniond q_R_SLAM2RTK(0.999993, 0.00118686, 0.00030921, 0.00352749); // w, x, y, z

    double d_time = 0.0;
    Eigen::Vector3d v_pos_slam(0.0, 0.0, 0.0);
    Eigen::Quaterniond q_rot_slam = Eigen::Quaterniond::Identity();
    Eigen::Vector3d v_vel_slam(0.0, 0.0, 0.0);

    size_t lidar_size0 = 0;
    size_t lidar_size1 = 0;
    size_t imu_size0 = 0;
    size_t imu_size1 = 0;
    size_t imu2_size0 = 0;
    size_t imu2_size1 = 0;
    size_t imu2_i0 = 0;
    size_t imu2_i1 = 0;
    size_t motor_size0 = 0;
    size_t motor_size1 = 0;

    double d_time_pre = 0.0;
    size_t count_all = 0;
    size_t count_update = 0;

    Eigen::Vector3d v_pos_slam_rtk(0.0, 0.0, 0.0);
    Eigen::Quaterniond q_rot_slam_rtk = Eigen::Quaterniond::Identity();
    Eigen::Vector3d v_vel_slam_rtk(0.0, 0.0, 0.0);
    Eigen::Vector3d v_vel_slam_local(0.0, 0.0, 0.0);
    double yaw = 0.0;
    double pitch = 0.0;
    double roll = 0.0;
    double vel = 0.0;

    double st = 0;
    bool res = false;
    dy_lio_pkg::rtk_msg rtk;
    geometry_msgs::PoseStamped pose;
    nav_msgs::Path path;
    ros::Rate rate(50);
    while (ros::ok())
    {
        st = ros::Time::now().toSec();
        ros::spinOnce();

        if (b_fin)
            break;
        if (!b_run)
            continue;

        // lioptr->CheckBuffers(lidar_size0, imu_size0, imu2_size0, imu2_i0);
        res = lioptr->GenerateResults(d_time, v_pos_slam, q_rot_slam, v_vel_slam);

        if (res)
        {
            // lioptr->CheckBuffers(lidar_size1, imu_size1, imu2_size1, imu2_i1);
            count_all++;
            if (d_time != d_time_pre)
            {
                count_update++;
                d_time_pre = d_time;
            }

            // v_pos_slam_rtk = q_R_SLAM2RTK * (q_rot_slam * v_P_RTKL_O_SLAML + v_pos_slam) + v_t_SLAM2RTK;
            // q_rot_slam_rtk = q_R_SLAM2RTK * q_rot_slam;
            // v_vel_slam_rtk = q_R_SLAM2RTK * v_vel_slam;
            // v_vel_slam_local = q_rot_slam.conjugate() * v_vel_slam;

            v_pos_slam_rtk = q_rot_slam * v_P_RTKL_O_SLAML + v_pos_slam;
            q_rot_slam_rtk = q_rot_slam;
            v_vel_slam_rtk = v_vel_slam;
            v_vel_slam_local = q_rot_slam.conjugate() * v_vel_slam;

            quat2EulAng(q_rot_slam_rtk.normalized(), roll, pitch, yaw);
            yaw = 90.0 - yaw * 180.0 / M_PI;
            if (yaw < 0.0)
                yaw += 360.0;

            prev_motor = cur_motor;
            mtx.lock();
            motor_size0 = que_motor.size();
            for (size_t i = 0; i < que_motor.size(); i++)
            {
                if (que_motor.front()(0) <= d_time)
                {
                    cur_motor = que_motor.front();
                    que_motor.pop();
                }
                else
                {
                    break;
                }
            }
            motor_size1 = que_motor.size();
            mtx.unlock();
            vel = kalman_filter(cur_motor(1) - prev_motor(1), v_vel_slam_local(0));

            rtk.header.stamp = ros::Time().fromSec(d_time);
            rtk.header.frame_id = std::string("wgs84_imuRaw");
            rtk.ee = v_pos_slam_rtk(0);
            rtk.nn = v_pos_slam_rtk(1);
            rtk.uu = v_pos_slam_rtk(2);
            rtk.lat = 0.0;
            rtk.lon = 0.0;
            rtk.alt = 0.0;
            rtk.yaw = yaw;
            rtk.pitch = 0.0;
            rtk.roll = 0.0;
            rtk.gyro_x = 0.0;
            rtk.gyro_y = 0.0;
            rtk.gyro_z = 0.0;
            rtk.acc_x = 0.0;
            rtk.acc_y = 0.0;
            rtk.acc_z = 0.0;
            rtk.ve = 0.0;
            rtk.vn = 0.0;
            rtk.vu = 0.0;
            rtk.v = vel;

            pose.header.stamp = ros::Time().fromSec(d_time);
            pose.header.frame_id = "map";
            pose.pose.position.x = v_pos_slam_rtk.x();
            pose.pose.position.y = v_pos_slam_rtk.y();
            pose.pose.position.z = v_pos_slam_rtk.z();
            pose.pose.orientation.w = q_rot_slam_rtk.w();
            pose.pose.orientation.x = q_rot_slam_rtk.x();
            pose.pose.orientation.y = q_rot_slam_rtk.y();
            pose.pose.orientation.z = q_rot_slam_rtk.z();

            path.poses.push_back(pose);
            path.header.stamp = ros::Time::now();
            path.header.frame_id = "map";

            pub_rtk.publish(rtk);
            pub_pose.publish(pose);
            pub_path.publish(path);

            mtx.lock();
            // std::cout << "----";
            // std::cout << lidar_size0 << " -> " << lidar_size1 << ",  ";
            // std::cout << imu_size0 << " -> " << imu_size1 << ",  ";
            // std::cout << imu2_size0 << " (" << imu2_i0 << ") -> " << imu2_size1 << " (" << imu2_i1 << ")\n";
            // std::cout << std::fixed << std::setprecision(3) << "----" << d_time << ",  ";
            // std::cout << ros::Time::now().toSec() << " (" << ros::Time::now().toSec() - st << "),  ";
            // std::cout << motor_size0 << " -> " << motor_size1 << ",  ";
            // std::cout << count_update << " / " << count_all << " (" << double(count_update) / double(count_all) << ")" << std::endl;
            mtx.unlock();
        }

        rate.sleep();
    }
    std::cout << "publish_results() fin" << std::endl;
}

/** main entry
 *
 */
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "dy_lio_pkg_node");
    ros::NodeHandle nh("~");

    init_parameters(nh);

    // zm--------------------------------------------------------------------------------------------------
    std::vector<float> vec_map_min{-26.7, -14.9, -6.0,
                                   -31.5, -26.1, -6.0,
                                   -36.3, -36.9, -6.0,
                                   -41.5, -48.1, -6.0,
                                   -41.5, -58.9, -6.0,
                                   -41.5, -69.9, -6.0,
                                   -41.5, -91.9, -6.0};
    std::vector<float> vec_map_max{11.6, -13.9, 4.0,
                                   16.6, -24.7, 4.0,
                                   21, -35.9, 4.0,
                                   25.8, -46.9, 4.0,
                                   30.4, -57.9, 4.0,
                                   35, -68.7, 4.0,
                                   44.6, -90.7, 4.0}; // 云南7堵墙坐标
    size_t num_area{7};
    // zm--------------------------------------------------------------------------------------------------

    // initialize iVox
    slam::IVoxType::Options ivox_options;
    ivox_options.resolution_ = 0.2;
    ivox_options.nearby_type_ = slam::IVoxType::NearbyType::NEARBY18;
    ivox = std::make_shared<slam::IVoxType>(ivox_options);

    // loading static map
    /*pcl::PCLPointCloud2 pcl_cld_stmap;
    pcl::io::loadPCDFile(map_file_name, pcl_cld_stmap);
    std::cout << pcl_cld_stmap.height << ", " << pcl_cld_stmap.width << std::endl;
    sensor_msgs::PointCloud2 msg_cld_stmap;
    pcl_conversions::moveFromPCL(pcl_cld_stmap, msg_cld_stmap);
    msg_cld_stmap.header.frame_id = "map";
    std::cout << msg_cld_stmap.height << ", " << msg_cld_stmap.width << std::endl;
    slam::PointCloudType::Ptr cldptr_stmap = boost::make_shared<slam::PointCloudType>();
    pcl::fromROSMsg(msg_cld_stmap, *cldptr_stmap);
    std::cout << cldptr_stmap->height << ", " << cldptr_stmap->width << std::endl;
    ivox->AddPoints(cldptr_stmap->points);
    std::cout << "add static map successfully" << std::endl;*/

    // loading static map
    slam::PointCloudType::Ptr cldptr_stmap(new slam::PointCloudType);
    std::cout << "map_file_name: " << map_file_name << std::endl;
    if (pcl::io::loadPCDFile<slam::PointType>(map_file_name, *cldptr_stmap) == -1)
    {
        std::cout << "load map error" << std::endl;
        return 0;
    }
    std::cout << "cldptr_stmap size: " << cldptr_stmap->size() << std::endl;
    //
    sensor_msgs::PointCloud2 msg_cld_stmap;
    pcl::toROSMsg(*cldptr_stmap, msg_cld_stmap);
    msg_cld_stmap.header.frame_id = "map";
    //
    //  ivox->IVOX_get_areas(vec_map_min, vec_map_max, num_area);
    //
    ivox->Mapping_AddPoints(cldptr_stmap->points);
    std::cout << "add static map successfully" << std::endl;

    ros::Subscriber motor_subscriber = nh.subscribe<geometry_msgs::PointStamped>(motor_topic_name, 2000, motor_callback);
    ros::Subscriber imu_subscriber = nh.subscribe<sensor_msgs::Imu>(imu_topic_name, 2000, imu_callback);
    ros::Subscriber pointcloud_subscriber = nh.subscribe<sensor_msgs::PointCloud2>(pointcloud_topic_name, 200, pointcloud_callback);

    ros::Publisher static_map_publisher = nh.advertise<sensor_msgs::PointCloud2>(static_map_topic_name, 1);
    ros::Publisher undistort_pointcloud_publisher = nh.advertise<sensor_msgs::PointCloud2>(undistort_pointcloude_topic_name, 1);
    ros::Publisher pose_raw_publisher = nh.advertise<geometry_msgs::PoseStamped>(slam_pose_raw_topic_name, 1);
    ros::Publisher path_raw_publisher = nh.advertise<nav_msgs::Path>(slam_path_raw_topic_name, 1);
    ros::Publisher prv_raw_publisher = nh.advertise<dy_lio_pkg::rtk_msg>(slam_prv_raw_topic_name, 1);

    ros::Publisher rtk_publisher = nh.advertise<dy_lio_pkg::rtk_msg>(slam_rtk_topic_name, 1);
    ros::Publisher pose_publisher = nh.advertise<geometry_msgs::PoseStamped>(slam_pose_topic_name, 1);
    ros::Publisher path_publisher = nh.advertise<nav_msgs::Path>(slam_path_topic_name, 1);

    ros::Publisher safety_publisher = nh.advertise<geometry_msgs::Vector3Stamped>(slam_safety_topic_name, 1);
    ros::Publisher state_publisher = nh.advertise<geometry_msgs::Vector3Stamped>(slam_state_topic_name, 1);

    // zm--------------------------------------------------------------------------------------------------
    ros::Publisher ivox_cld_publisher = nh.advertise<sensor_msgs::PointCloud2>("/ivox_pcl_cld", 1);
    // zm--------------------------------------------------------------------------------------------------

    ros::Rate rate(200);

    // calculate the init pos and ori
    Eigen::Matrix4f mat_init;
    cldreg::Reg cld_reg(cldptr_stmap, 1.0, 10);
    while (ros::ok())
    {
        ros::spinOnce();
        if (cldptr_init)
        {
            if (cldptr_init->size() != 0)
                break;
        }
        rate.sleep();
    }
    if (!cldptr_init)
    {
        std::cout << "subscribe cld error" << std::endl;
        return 0;
    }
    if (cldptr_init->size() == 0)
    {
        std::cout << "subscribe cld error" << std::endl;
        return 0;
    }
    cld_reg.getFinalTransformation(cldptr_init, mat_init);
    std::cout << mat_init << std::endl;

    // initialize LIO
    lioptr = std::make_shared<slam::Lio>(ivox, mat_init.cast<double>());
    b_run = true;

    std::thread thrd_safety(publish_safety, std::ref(safety_publisher), std::ref(state_publisher));
    usleep(100000);
    std::thread thrd_results(publish_results, std::ref(rtk_publisher), std::ref(pose_publisher), std::ref(path_publisher));

    //std::thread thrd_ivox_cld(publish_ivox_cld, std::ref(ivox_cld_publisher));

    std::cout << std::this_thread::get_id() << std::endl;
    std::cout << thrd_safety.get_id() << std::endl;
    std::cout << thrd_results.get_id() << std::endl;
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(7, &cpuset);
    int rc_main = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
    CPU_ZERO(&cpuset);
    CPU_SET(6, &cpuset);
    int rc_safety = pthread_setaffinity_np(thrd_safety.native_handle(), sizeof(cpu_set_t), &cpuset);
    int rc_results = pthread_setaffinity_np(thrd_results.native_handle(), sizeof(cpu_set_t), &cpuset);
    if (rc_main != 0 || rc_safety != 0 || rc_results != 0)
    {
        std::cout << "error calling pthread_setaffinity_np: " << rc_main << ", " << rc_safety << ", " << rc_results << std::endl;
    }

    int res = 0;
    Eigen::Vector3d pos(0.0, 0.0, 0.0);
    Eigen::Quaterniond rot = Eigen::Quaterniond::Identity();
    Eigen::Vector3d vel(0.0, 0.0, 0.0);
    geometry_msgs::PoseStamped pose;
    nav_msgs::Path path;
    dy_lio_pkg::rtk_msg prv;
    geometry_msgs::Vector3Stamped msg_state;
    while (ros::ok())
    {
        ros::spinOnce();

        slam::PointCloudType::Ptr cldptr_slam = nullptr;
        res = lioptr->Run(cldptr_slam, pos, rot, vel);

        if (res == 0)
            continue;
        else if (res == 1)
            continue;
        else if (res == 2)
        {
            sensor_msgs::PointCloud2::Ptr msgptr_cld_slam = boost::make_shared<sensor_msgs::PointCloud2>();
            pcl::toROSMsg(*cldptr_slam, *msgptr_cld_slam);
            msgptr_cld_slam->header.frame_id = "map";

            pose.header.stamp = msgptr_cld_slam->header.stamp;
            pose.header.frame_id = "map";
            pose.pose.position.x = pos.x();
            pose.pose.position.y = pos.y();
            pose.pose.position.z = pos.z();
            pose.pose.orientation.w = rot.w();
            pose.pose.orientation.x = rot.x();
            pose.pose.orientation.y = rot.y();
            pose.pose.orientation.z = rot.z();

            path.poses.push_back(pose);
            path.header.stamp = ros::Time::now();
            path.header.frame_id = "map";

            prv.header.stamp = msgptr_cld_slam->header.stamp;
            prv.header.frame_id = "map";
            prv.ee = pos.x();
            prv.nn = pos.y();
            prv.uu = pos.z();
            prv.gyro_x = rot.x();
            prv.gyro_y = rot.y();
            prv.gyro_z = rot.z();
            prv.acc_x = rot.w();
            prv.ve = vel.x();
            prv.vn = vel.y();
            prv.vu = vel.z();
            prv.v = vel.norm();

            //static_map_publisher.publish(msg_cld_stmap);
            //undistort_pointcloud_publisher.publish(msgptr_cld_slam);
            pose_raw_publisher.publish(pose);
            //path_raw_publisher.publish(path);
            prv_raw_publisher.publish(prv);

            n_state = 1;
            msg_state.header.stamp = ros::Time::now();
            msg_state.vector.x = n_state;
            state_publisher.publish(msg_state);
        }
        else if (res == -1)
        {
            n_state = 0;
            msg_state.header.stamp = ros::Time::now();
            msg_state.vector.x = n_state;
            state_publisher.publish(msg_state);

            double d_st = ros::Time::now().toSec();
            que_fail.push(d_st);
            d_st -= 60.0;
            for (size_t i = 0; i < que_fail.size(); i++)
            {
                if (que_fail.front() < d_st)
                    que_fail.pop();
                else
                    break;
            }
            if (que_fail.size() >= 3)
            {
                std::cout << "localization error: exceeded maximum number of retries" << std::endl;
                break;
            }

            std::cout << "wait ... " << std::endl;
            usleep(3000000);
            for (size_t i = 0; i < 1000; i++)
                ros::spinOnce();
            std::cout << "wait fin, restart" << std::endl;

            b_run = false;
            cldptr_init = nullptr;
            while (ros::ok())
            {
                ros::spinOnce();
                if (cldptr_init)
                {
                    if (cldptr_init->size() != 0)
                        break;
                }
                rate.sleep();
            }
            if (!cldptr_init)
            {
                std::cout << "subscribe cld error" << std::endl;
                break;
            }
            if (cldptr_init->size() == 0)
            {
                std::cout << "subscribe cld error" << std::endl;
                break;
            }
            cld_reg.getFinalTransformation(cldptr_init, mat_init);
            std::cout << mat_init << std::endl;

            lioptr = std::make_shared<slam::Lio>(ivox, mat_init.cast<double>());
            b_run = true;

            // break;
        }

        rate.sleep();
    }
    std::cout << "while() fin" << std::endl;
    b_fin = true;

    thrd_safety.join();
    thrd_results.join();
    std::cout << "main() fin" << std::endl;

    return 0;
}
